#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "Motor.h"
#include "Distance.h"
#include "AD.h"
#include "Angle.h"
#include "PWM.h"
#include "Pid.h"
#include "Switcher.h"
#include "Light.h"
#include <stdlib.h>
#include <math.h>
#include <IC.h>
extern float* Tank;
 uint8_t i;
 uint16_t Num;
 extern float current_angle ;
int main(void)
{
	Tank = (float*)malloc(2 * sizeof(float));   //给指针分配指向的内存空间
	OLED_Init();		
	Motor_Init () ;
	Key_Init ();
  float* Tem_Dot1 ;
  float* Tem_Dot2 ;
  OLED_ShowString(1,1,"Hallo");
	Motor_SetSpeed(0);
	while(1)
	{
OLED_ShowNum(3,1,tdoaLsSolve( (cha (arr_pin1 ,arr_pin2,175,128))*55.5/72000000,  (cha (arr_pin1 ,arr_pin0,175,128))*55.5/72000000,(cha (arr_pin1 ,arr_pin3,175,128))*55.5/72000000,Tank)*100,4);		if(Key_Function ()  == 0)  //模式1
			{
				float angle = Angle (); //利用tank计算目标角度
				Light_Up_Judger(angle); //对应位置LED点亮
				Delay_s(1); 				//一秒之后电机转动
				while(angle - current_angle > 3 ||angle - current_angle < -3)
				{
					Pid_Function_Wait (angle);//可以实现当前角度传递+方向控制  且不断调整 
				}      
				 PWM_SetCompare4(0);//小于误差范围停止调整
				 	 OLED_ShowNum(3,1,tdoaLsSolve( (cha (arr_pin1 ,arr_pin2,175,128))*55.5/72000000,  (cha (arr_pin1 ,arr_pin0,175,128))*55.5/72000000,(cha (arr_pin1 ,arr_pin3,175,128))*55.5/72000000,Tank)*1000,4);

				while(Key_Function () == 0)//此状态下等待  
				{
				}
				
			
			}
			
			else if(Key_Function () == 1) //模式2  电机灯环持续定位
			{
				Light_Up_Judger(Angle()); //对应位置LED点亮 且目标角度实时调整 
				while(TIM_GetCapture2(TIM3) != 0)
				{	
				Pid_Function_Wait (Angle ());           //内部完成，节省
				}//可以实现当前角度传递+方向控制  调好直接跳出取消抖动
        	while(Key_Function () == 1)//此状态下等待  
				{
				}			
			}
			
			
			
			else if (Key_Function () == 2)   //模式3
			{ 
					Tem_Dot1 = (float*)malloc(2 * sizeof(float));   //给指针分配指向的内存空间
	        Tem_Dot2 = (float*)malloc(2 * sizeof(float));   //防止非法覆盖，导致其他数据丢失
				OLED_Clear();
				OLED_ShowString(1,1,"Distance:") ;
				OLED_ShowNum(1,10,tdoaLsSolve( (cha (arr_pin1 ,arr_pin2,175,128))*55.5/72000000,  (cha (arr_pin1 ,arr_pin0,175,128))*55.5/72000000,(cha (arr_pin1 ,arr_pin3,175,128))*55.5/72000000,Tank),4);//所间隔的tau乘以采样时间)
			  if(Key_Function2 () == 1)
				{
					Tem_Dot1[0] = Tank[0];
					Tem_Dot1[1] = Tank[1];
					OLED_ShowString(2,1,"No1:Ready!");	
				}
				else if(Key_Function2 () == 2)
				{
					Tem_Dot2[0] = Tank[0];
					Tem_Dot2[1] = Tank[1];
					OLED_ShowString(3,1,"No2:Ready!");	
				}	
				float Divi_Distance = sqrt((Tem_Dot1[0] -  Tem_Dot2[0])* (Tem_Dot1[0] -  Tem_Dot2[0]) +  (Tem_Dot1[1] - Tem_Dot2[1])*(Tem_Dot1[1] - Tem_Dot2[1]));
			  free(Tem_Dot1);
				free(Tem_Dot2);
        Tem_Dot1 = NULL;
				Tem_Dot2 = NULL;
				OLED_ShowNum(4,1,Divi_Distance,8) ;
					
				 while(Key_Function () == 2)//模式三下等待  
				{
				}
			}                                                       //模式4 计算打印距离+   
			
			else
			{
				float angle = Angle (); //利用tank计算目标角度
				Light_Up_Judger(angle); //对应位置LED点亮
				Delay_s(1); 				//一秒之后电机转动
			while(TIM_GetCapture2(TIM3) != 0)
				{	
				Pid_Function_Wait (Angle ());           //内部完成，节省
				}//可以实现当前角度传递+方向控制  调好直接跳出取消抖动
       	
				while(Key_Function () ==3)//此状态下等待                                  //暂时与模式一相同，可能后续要加上信号处理相关函数
				{
				}
			}
				

	}

			
}


